
#!/usr/bin/python
from ctypes import *



#参考：https://blog.csdn.net/weixin_39704374/article/details/111450185
#https://blog.csdn.net/mak0000/article/details/82227515
#https://docs.python.org/zh-cn/3/library/ctypes.html
class MiniBotAlgorithmLib:
   '导入动态库'
   lib = cdll.LoadLibrary("./MiniBotAlgorithmLib.x64.dll")
   #def __init__(self):
   #   self.lib = cdll.LoadLibrary("./MiniBotAlgorithmLib.x64.dll")
   #   return
    
   #计算关节角度To末端笛卡尔空间
   #参数： j[5] 五元素的数组指针对于I T T I T (G) 的关节角度值 单位是弧度角 转换为角度单位后值范围
   #I：正负150角度,T：正负90，G：-90到0 （0度的关节模块0位）
   #输出：笛卡尔空间 X Y Z Roll Pitch Yaw 六个元素的数组
   def CalJointAngleToTerminalCartesian(self,angles):

      self.lib.CalJointAngleToTerminalCartesian.restype= POINTER(c_float)
      size = len(angles)
      anglesp = (c_float*size)()
      for i in range(size):
         anglesp[i]=(c_float(angles[i])) 
      r = self.lib.CalJointAngleToTerminalCartesian(anglesp)
      xyzRPY = []
      for i in range(6):
         xyzRPY.append(r[i]) 
      return xyzRPY

   #计算末端笛卡尔空间To关节角度
   #参数： pose[6] X Y Z Roll Pitch Yaw 六个元素的数组
   #输出：关节弧度角 float* 五元素的数组指针对于I T T I T (G) 的关节角度值 单位是弧度角 转换为角度单位后值范围
   #I：正负150角度,T：正负90，G：-90到0 （0度的关节模块0位）
   def CalTerminalCartesianToJointAngle(self,xyzRPY):
      self.lib.CalTerminalCartesianToJointAngle.restype= POINTER(c_float)
      size = len(xyzRPY)
      xyzRPYp =(c_float*size)()
      for i in range(size):
         xyzRPYp[i]=(c_float(xyzRPY[i])) 
      r = self.lib.CalTerminalCartesianToJointAngle(xyzRPYp)
    
      angles = []
      for i in range(5):
         angles.append(r[i]) 
      return angles

    #<summary>
    #添加机器人
    #</summary>
    #<param name="robotType">机器人类别：1遥控小车、2蛇、3双足/双轮、4攀爬、5仿生蜘蛛、6、X轮式</param>
    #<param name="ids">
    #第一个ID均为与控制底座连接的模块ID，如多个以下分开说明，位置请参考文档说明
    #攀爬：T:Down,G:Down,T:Up,G:Up
    #双足/双轮：T:Down,T:Down,IU:Down,T:Up,T:Up,IU:Up
    #</param>
    #<param name="count"></param>
    #<returns>返回机器人控制ID</returns>
   def AddRobot(self,robotType,ids):
    size=len(ids)
    idTypes=c_ubyte*size
    idsp =idTypes()
    
    #charPP = POINTER(c_char_p)
    #idsValue = c_char_p(size)
    #idsP = charPP(idsValue)  
    for i in range(size):
       idsp[i]=(c_ubyte(ids[i]))
    r = self.lib.AddRobot(robotType,idsp,c_int(size))
    return r

   #<summary>
   #移除机器人
   #</summary>
   #<param name="id">机器Id标识</param>
   #<returns></returns>
   def RemoveRobot(self,id):
    r = self.lib.RemoveRobot(c_int(id))
    return r

   #<summary>
   #对指定机器人发送指令
   #</summary>
   #<param name="id">机器人ID</param>
   #<param name="cmd">命令类别</param>
   #<param name="parameter">命令参数</param>
   #<returns></returns>
   def SendCmd(self,id, cmd, parameter):
    r = self.lib.SendCmd(c_int(id),c_int(cmd),c_int(parameter))
    return r

   #<summary>
   #绑定模块控制接口
   #</summary>
   #<param name="setPosition"></param>
   #<param name="setPositionWithSpeed"></param>
   #<param name="setMultiPosition"></param>
   #<param name="setMultiPositionWithSpeed"></param>
   #<param name="setSpeed"></param>
   #<param name="setRunningSpeed"></param>
   #<param name="enableTorque"></param>
   #<param name="setMode"></param>
   #<param name="threadSleep"></param>
   #https://stackoverflow.com/questions/37973033/ctypes-python3-5-oserror-exception-access-violation-writing-0x00000000
   def BindRobotModuleApis(self,id, setPositionCallback,setPositionWithSpeedCallback,setMultiPositionCallback,setMultiPositionWithSpeedCallback,setPositionSpeedCallback,setRunningSpeedCallback,torqueEnableCallback,setModeCallback):
  
    #通过此种方式在此处定义，外部调用此方法后，函数地址会被回收，在回调时会引发：exception: access violation reading 0x0000000000000005 异常
    #setPositionCallback_type = CFUNCTYPE(None,c_ubyte, c_double)
    #setPositionWithSpeedCallback_type = CFUNCTYPE(None,c_ubyte, c_double,c_int)
    #setMultiPositionCallback_type = CFUNCTYPE(None,POINTER(c_ubyte), POINTER(c_double),c_int)
    #setMultiPositionWithSpeedCallback_type = CFUNCTYPE(None,POINTER(c_ubyte), POINTER(c_double),c_int , c_int)
    
    #setSpeedCallback_type = CFUNCTYPE(None,c_ubyte, c_int)
    #setTorqueEnableCallback_type = CFUNCTYPE(None,c_ubyte, c_bool)
    #setModeCallback_type = CFUNCTYPE(None,c_ubyte, c_int)

    #setPosition = setPositionCallback_type(setPositionCallback)
    #setPositionWithSpeed = setPositionWithSpeedCallback_type(setPositionWithSpeedCallback)
    #setMultiPosition = setMultiPositionCallback_type(setMultiPositionCallback)
    #setMultiPositionWithSpeed = setMultiPositionWithSpeedCallback_type(setMultiPositionWithSpeedCallback)

    #setPositionSpeed = setSpeedCallback_type(setPositionSpeedCallback)
    #setRunningSpeed = setSpeedCallback_type(setRunningSpeedCallback)
    #setTorqueEnable = setTorqueEnableCallback_type(torqueEnableCallback)
    #setMode = setModeCallback_type(setModeCallback)
    #通过此种方式在此处定义，外部调用此方法后，函数地址会被回收，在回调时会引发：exception: access violation reading 0x0000000000000005 异常
    r = self.lib.BindRobotModuleApis(c_int(id),setPositionCallback,setPositionWithSpeedCallback,setMultiPositionCallback,setMultiPositionWithSpeedCallback,setPositionSpeedCallback,setRunningSpeedCallback,torqueEnableCallback,setModeCallback)
    return r
